<HTML> 
<HEAD> 
	<TITLE>3rd Party ROBOTC Drivers</TITLE> 
<link href="tabs.css" rel="stylesheet" type="text/css"/> 
<link href="doxygen.css" rel="stylesheet" type="text/css"/> 
</HEAD> 
 
<BODY bgcolor="#ffffff" link="#000000" vlink="#000000"> 
<table width="100%" bgcolor="navy" cellspacing=0 cellpadding=1 border=0> 
<tr><td><table width="100%" bgcolor="#EEEEDD" cellspacing=0 cellpadding=3 border=0> 
<tr> 
    <td width="33%" align="left"> <img src="images/LOGO_NXT.gif" width=266 height=44 border="0" alt="Mindstorms"></td> 
    <td><b><font size="+3" color="navy">3rd Party ROBOTC Drivers</font></b></td> 
    <td align="right"> <img src="images/logo.png" width=44 height=44 border="0" alt="RobotC"></td>     
</tr> 
  <tr bgcolor="#cccc99" align="right"> 
          <td colspan=3> <font face="arial" size="-1"> [<a href=main.html>Home</a>] [<a target=_top href="https://sourceforge.net/projects/rdpartyrobotcdr/">Download</a>] [<a target=_top href="http://apps.sourceforge.net/mantisbt/rdpartyrobotcdr/my_view_page.php">Submit a bug/suggestion</a>]  [<a target=_top href="http://www.robotc.net/forums/">ROBOTC Forums</a>] [<a target=_top href="http://botbench.com">Blog</a>] [<a target=_top href="http://sourceforge.net/donate/index.php?group_id=257238">Support this project</a>]</font> </td> 
</tr> 
</table></td></tr> 
</table> 
<!-- Generated by Doxygen 1.7.2 -->
<div class="header">
  <div class="headertitle">
<h1>mindsensors-nxtcam.h</h1>  </div>
</div>
<div class="contents">
<a href="mindsensors-nxtcam_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*!@addtogroup mindsensors</span>
<a name="l00002"></a>00002 <span class="comment"> * @{</span>
<a name="l00003"></a>00003 <span class="comment"> * @defgroup nxtcam NXTCam Vision System</span>
<a name="l00004"></a>00004 <span class="comment"> * NXTCam Vision System</span>
<a name="l00005"></a>00005 <span class="comment"> * @{</span>
<a name="l00006"></a>00006 <span class="comment"> */</span>
<a name="l00007"></a>00007 
<a name="l00008"></a>00008 <span class="comment">/*</span>
<a name="l00009"></a>00009 <span class="comment"> * $Id: mindsensors-nxtcam.h 133 2013-03-10 15:15:38Z xander $</span>
<a name="l00010"></a>00010 <span class="comment"> */</span>
<a name="l00011"></a>00011 
<a name="l00012"></a>00012 <span class="preprocessor">#ifndef __NXTCAM_H__</span>
<a name="l00013"></a>00013 <span class="preprocessor"></span><span class="preprocessor">#define __NXTCAM_H__</span>
<a name="l00014"></a>00014 <span class="preprocessor"></span><span class="comment">/** \file mindsensors-nxtcam.h</span>
<a name="l00015"></a>00015 <span class="comment"> * \brief Mindsensors NXTCam driver</span>
<a name="l00016"></a>00016 <span class="comment"> *</span>
<a name="l00017"></a>00017 <span class="comment"> * mindsensors-nxtcam.h provides an API for the Mindsensors NXTCam.  This version is an extensive rewrite of</span>
<a name="l00018"></a>00018 <span class="comment"> * Gordon Wyeth&#39;s driver. The blob information is no longer kept in seperate array,</span>
<a name="l00019"></a>00019 <span class="comment"> * but in an array of structs.</span>
<a name="l00020"></a>00020 <span class="comment"> *</span>
<a name="l00021"></a>00021 <span class="comment"> * Changelog:</span>
<a name="l00022"></a>00022 <span class="comment"> * - 1.0: Partial rewrite of original driver, using structs instead of arrays to hold blob info</span>
<a name="l00023"></a>00023 <span class="comment"> * - 1.1: Further rewrite to use common.h functions to further align with standard driver framework</span>
<a name="l00024"></a>00024 <span class="comment"> * - 1.2: Added NXTCAMgetCenter() to calculate the center of a single blob</span>
<a name="l00025"></a>00025 <span class="comment"> * - 1.3: Added NXTCAMinitTL() to enable line tracking mode&lt;br&gt;</span>
<a name="l00026"></a>00026 <span class="comment"> *        Fixed bug in NXTCAMinit() that did not configure object tracking properly&lt;br&gt;</span>
<a name="l00027"></a>00027 <span class="comment"> *        Added extra wait times after each issued command in init functions</span>
<a name="l00028"></a>00028 <span class="comment"> * - 1.4: Removed printDebugLine from driver</span>
<a name="l00029"></a>00029 <span class="comment"> * - 1.5: Added ability to specify I2C address with optional argument.  Defaults to 0x02 when not specified.</span>
<a name="l00030"></a>00030 <span class="comment"> *</span>
<a name="l00031"></a>00031 <span class="comment"> * License: You may use this code as you wish, provided you give credit where it&#39;s due.</span>
<a name="l00032"></a>00032 <span class="comment"> *</span>
<a name="l00033"></a>00033 <span class="comment"> * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER. </span>
<a name="l00034"></a>00034 <span class="comment"></span>
<a name="l00035"></a>00035 <span class="comment"> * \author Xander Soldaat</span>
<a name="l00036"></a>00036 <span class="comment"> * \author Gordon Wyeth</span>
<a name="l00037"></a>00037 <span class="comment"> * \date 03 Dec 2010</span>
<a name="l00038"></a>00038 <span class="comment"> * \version 1.5</span>
<a name="l00039"></a>00039 <span class="comment"> * \example mindsensors-nxtcam-test1.c</span>
<a name="l00040"></a>00040 <span class="comment"> */</span>
<a name="l00041"></a>00041 
<a name="l00042"></a>00042 <span class="preprocessor">#pragma systemFile</span>
<a name="l00043"></a>00043 <span class="preprocessor"></span>
<a name="l00044"></a>00044 <span class="preprocessor">#ifndef __COMMON_H__</span>
<a name="l00045"></a>00045 <span class="preprocessor"></span><span class="preprocessor">#include &quot;<a class="code" href="common_8h.html" title="Commonly used functions used by drivers.">common.h</a>&quot;</span>
<a name="l00046"></a>00046 <span class="preprocessor">#endif</span>
<a name="l00047"></a>00047 <span class="preprocessor"></span>
<a name="l00048"></a><a class="code" href="group__nxtcam.html#ga3104c35d33ecdfdcaa2af97f1691df8a">00048</a> <span class="preprocessor">#define MAX_BLOBS         8     </span><span class="comment">/*!&lt; Maximum number of blobs returned by the NXTCam */</span>
<a name="l00049"></a><a class="code" href="group__nxtcam.html#ga36fbe48f1799ef0af9efab553c0bd9c9">00049</a> <span class="preprocessor">#define NXTCAM_I2C_ADDR   0x02  </span><span class="comment">/*!&lt; I2C address used by the NXTCam */</span>
<a name="l00050"></a><a class="code" href="group__nxtcam.html#gad16acce76c61ecff2aec7e38ba6537cf">00050</a> <span class="preprocessor">#define NXTCAM_CMD_REG    0x41  </span><span class="comment">/*!&lt; Register used for issuing commands */</span>
<a name="l00051"></a><a class="code" href="group__nxtcam.html#ga87f344485cd003c785b54959c883b924">00051</a> <span class="preprocessor">#define NXTCAM_COUNT_REG  0x42  </span><span class="comment">/*!&lt; Register used to hold number of blobs detected */</span>
<a name="l00052"></a><a class="code" href="group__nxtcam.html#gadd6a460c504511474edf8a6f4e9687a6">00052</a> <span class="preprocessor">#define NXTCAM_DATA_REG   0x43  </span><span class="comment">/*!&lt; Register containing data pertaining to blobs */</span>
<a name="l00053"></a>00053 
<a name="l00054"></a><a class="code" href="group__nxtcam.html#gaac63b3236275d61b690cb17ea7634072">00054</a> <span class="preprocessor">#define SIDE_CENTER(X1, X2) ((X1 + X2) / 2)  </span><span class="comment">/*!&lt; Returns the center of a side */</span>
<a name="l00055"></a>00055 <span class="comment"></span>
<a name="l00056"></a>00056 <span class="comment">/*! Blob struct, contains all the data for a blob. */</span>
<a name="l00057"></a><a class="code" href="structblob.html">00057</a> <span class="keyword">typedef</span> <span class="keyword">struct </span>{
<a name="l00058"></a><a class="code" href="structblob.html#a97fcb0e1731b36905e90757f211b1340">00058</a>   <span class="keywordtype">int</span> <a class="code" href="structblob.html#a97fcb0e1731b36905e90757f211b1340">x1</a>;       <span class="comment">/*!&lt; left */</span>
<a name="l00059"></a><a class="code" href="structblob.html#a886daa12d11655f342168e87c7c0b1cb">00059</a>   <span class="keywordtype">int</span> <a class="code" href="structblob.html#a886daa12d11655f342168e87c7c0b1cb">y1</a>;       <span class="comment">/*!&lt; top */</span>
<a name="l00060"></a><a class="code" href="structblob.html#ad660c90589efbb535c7d55dc032c1b09">00060</a>   <span class="keywordtype">int</span> <a class="code" href="structblob.html#ad660c90589efbb535c7d55dc032c1b09">x2</a>;       <span class="comment">/*!&lt; right */</span>
<a name="l00061"></a><a class="code" href="structblob.html#ab01055e87d61c94f71e59f1116867f60">00061</a>   <span class="keywordtype">int</span> <a class="code" href="structblob.html#ab01055e87d61c94f71e59f1116867f60">y2</a>;       <span class="comment">/*!&lt; bottom */</span>
<a name="l00062"></a><a class="code" href="structblob.html#a9fd0872cf541a7a85ff10b32bee4d34c">00062</a>   <span class="keywordtype">int</span> <a class="code" href="structblob.html#a9fd0872cf541a7a85ff10b32bee4d34c">colour</a>;   <span class="comment">/*!&lt; Blob colour */</span>
<a name="l00063"></a><a class="code" href="structblob.html#a439227feff9d7f55384e8780cfc2eb82">00063</a>   <span class="keywordtype">int</span> <a class="code" href="structblob.html#a439227feff9d7f55384e8780cfc2eb82">size</a>;     <span class="comment">/*!&lt; Blob size */</span>
<a name="l00064"></a>00064 } <a class="code" href="structblob.html">blob</a>;
<a name="l00065"></a>00065 <span class="comment"></span>
<a name="l00066"></a>00066 <span class="comment">/*! Array of blob as a typedef, this is a work around for RobotC&#39;s inability to pass an array to a function */</span>
<a name="l00067"></a><a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">00067</a> <span class="keyword">typedef</span> <a class="code" href="structblob.html">blob</a> <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a>[<a class="code" href="group__nxtcam.html#ga3104c35d33ecdfdcaa2af97f1691df8a">MAX_BLOBS</a>];
<a name="l00068"></a>00068 
<a name="l00069"></a><a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">00069</a> <a class="code" href="group__common.html#gaa172ef131853460e5b9f5c6db81d30f4">tByteArray</a> <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>;    <span class="comment">/*!&lt; Array to hold I2C command data */</span>
<a name="l00070"></a><a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">00070</a> <a class="code" href="group__common.html#gaa172ef131853460e5b9f5c6db81d30f4">tByteArray</a> <a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">NXTCAM_I2CReply</a>;      <span class="comment">/*!&lt; Array to hold I2C reply data */</span>
<a name="l00071"></a>00071 
<a name="l00072"></a>00072 <span class="comment">// &quot;public&quot; functions</span>
<a name="l00073"></a>00073 <span class="keywordtype">bool</span> <a class="code" href="group__nxtcam.html#gaa9503a35f45a1ffa5b298c65e95badf0">NXTCAMinit</a>(tSensors link, ubyte address = <a class="code" href="group__nxtcam.html#ga36fbe48f1799ef0af9efab553c0bd9c9">NXTCAM_I2C_ADDR</a>);
<a name="l00074"></a>00074 <span class="keywordtype">bool</span> <a class="code" href="group__nxtcam.html#ga47cb3406f218eeb9e9711d0a7184e410">NXTCAMinitTL</a>(tSensors link, ubyte address = <a class="code" href="group__nxtcam.html#ga36fbe48f1799ef0af9efab553c0bd9c9">NXTCAM_I2C_ADDR</a>);
<a name="l00075"></a>00075 <span class="keywordtype">int</span> <a class="code" href="group__nxtcam.html#ga5259766d6514ee73a08421ac88e77b12">NXTCAMgetBlobs</a>(tSensors link, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs, <span class="keywordtype">bool</span> mergeBlobs, ubyte address = <a class="code" href="group__nxtcam.html#ga36fbe48f1799ef0af9efab553c0bd9c9">NXTCAM_I2C_ADDR</a>);
<a name="l00076"></a>00076 <span class="keywordtype">int</span> <a class="code" href="group__nxtcam.html#ga5259766d6514ee73a08421ac88e77b12">NXTCAMgetBlobs</a>(tSensors link, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs, ubyte address = <a class="code" href="group__nxtcam.html#ga36fbe48f1799ef0af9efab553c0bd9c9">NXTCAM_I2C_ADDR</a>);
<a name="l00077"></a>00077 
<a name="l00078"></a>00078 <span class="comment">// internal functions, used by the above</span>
<a name="l00079"></a>00079 <span class="keywordtype">bool</span> <a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(tSensors link, byte cmd, ubyte address);
<a name="l00080"></a>00080 <span class="keywordtype">int</span> <a class="code" href="group__nxtcam.html#gadb70d2a4b3e24685060d2bcf95d9b473">_mergeBlobs</a>(<span class="keywordtype">int</span> blob1, <span class="keywordtype">int</span> blob2, <span class="keywordtype">int</span> nblobs, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs);
<a name="l00081"></a>00081 <span class="keywordtype">int</span> <a class="code" href="group__nxtcam.html#ga444410109e39138d52242c0570b5d0f9">_merge</a>(<span class="keywordtype">int</span> nblobs, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs);
<a name="l00082"></a>00082 <span class="keywordtype">void</span> <a class="code" href="group__nxtcam.html#ga1d6f5eddd1d996914310d608d031664e">_sortBlobs</a>(<span class="keywordtype">int</span> nblobs, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs);
<a name="l00083"></a>00083 <span class="keywordtype">void</span> <a class="code" href="group__nxtcam.html#ga177c2ae2e5fcc598179d2289b3a420df">NXTCAMgetAverageCenter</a>(<a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs, <span class="keywordtype">int</span> nblobs, <span class="keywordtype">int</span> colourindex, <span class="keywordtype">int</span> &amp;x, <span class="keywordtype">int</span> &amp;y);
<a name="l00084"></a>00084 <span class="keywordtype">void</span> <a class="code" href="group__nxtcam.html#ga6ca36236d45c9cdc7bbc56f266bdf6c6">NXTCAMgetCenter</a>(<a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs, <span class="keywordtype">int</span> index, <span class="keywordtype">int</span> &amp;x, <span class="keywordtype">int</span> &amp;y);
<a name="l00085"></a>00085 <span class="comment"></span>
<a name="l00086"></a>00086 <span class="comment">/*! boolean to signal if there are still blobs that might qualify for merging */</span>
<a name="l00087"></a><a class="code" href="group__nxtcam.html#ga02fd1071ae1b5e8665557ab086c33112">00087</a> <span class="keywordtype">bool</span> <a class="code" href="group__nxtcam.html#ga02fd1071ae1b5e8665557ab086c33112">still_merging</a> = <span class="keyword">false</span>;
<a name="l00088"></a>00088 <span class="comment"></span>
<a name="l00089"></a>00089 <span class="comment">/**</span>
<a name="l00090"></a>00090 <span class="comment"> * This function sends a command to the camera over I2C.</span>
<a name="l00091"></a>00091 <span class="comment"> *</span>
<a name="l00092"></a>00092 <span class="comment"> * Note: this is an internal function and should not be called directly.</span>
<a name="l00093"></a>00093 <span class="comment"> * @param link the sensor port number</span>
<a name="l00094"></a>00094 <span class="comment"> * @param cmd the command to be sent</span>
<a name="l00095"></a>00095 <span class="comment"> * @param address the I2C address to use, optional, defaults to 0x02</span>
<a name="l00096"></a>00096 <span class="comment"> * @return true if no error occured, false if it did</span>
<a name="l00097"></a>00097 <span class="comment"> */</span>
<a name="l00098"></a><a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">00098</a> <span class="keywordtype">bool</span> <a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(tSensors link, byte cmd, ubyte address) {
<a name="l00099"></a>00099   <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[0] = 3;                 <span class="comment">// Message size</span>
<a name="l00100"></a>00100   <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[1] = address;           <span class="comment">// I2C Address</span>
<a name="l00101"></a>00101   <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[2] = <a class="code" href="group__nxtcam.html#gad16acce76c61ecff2aec7e38ba6537cf">NXTCAM_CMD_REG</a>;    <span class="comment">// Register used for issuing commands</span>
<a name="l00102"></a>00102   <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[3] = cmd;               <span class="comment">// Command to be executed</span>
<a name="l00103"></a>00103 
<a name="l00104"></a>00104   <span class="keywordflow">return</span> <a class="code" href="group__common.html#gad02e06f83b264ee6fc7df41eb4712080">writeI2C</a>(link, <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>);
<a name="l00105"></a>00105 }
<a name="l00106"></a>00106 <span class="comment"></span>
<a name="l00107"></a>00107 <span class="comment">/**</span>
<a name="l00108"></a>00108 <span class="comment"> * This function initialises camera ready to find blobs and sort them according to size.</span>
<a name="l00109"></a>00109 <span class="comment"> * @param link the sensor port number</span>
<a name="l00110"></a>00110 <span class="comment"> * @param address the I2C address to use, optional, defaults to 0x02</span>
<a name="l00111"></a>00111 <span class="comment"> * @return true if no error occured, false if it did</span>
<a name="l00112"></a>00112 <span class="comment"> */</span>
<a name="l00113"></a><a class="code" href="group__nxtcam.html#gaa9503a35f45a1ffa5b298c65e95badf0">00113</a> <span class="keywordtype">bool</span> <a class="code" href="group__nxtcam.html#gaa9503a35f45a1ffa5b298c65e95badf0">NXTCAMinit</a>(tSensors link, ubyte address) {
<a name="l00114"></a>00114   <span class="keywordflow">if</span> (!<a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(link, <span class="charliteral">&#39;D&#39;</span>, address)) <span class="comment">// Stop object tracking</span>
<a name="l00115"></a>00115     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00116"></a>00116   wait1Msec(500);
<a name="l00117"></a>00117 
<a name="l00118"></a>00118   <span class="keywordflow">if</span> (!<a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(link, <span class="charliteral">&#39;A&#39;</span>, address)) <span class="comment">// Sort by size</span>
<a name="l00119"></a>00119     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00120"></a>00120   wait1Msec(500);
<a name="l00121"></a>00121 
<a name="l00122"></a>00122   <span class="keywordflow">if</span> (!<a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(link,<span class="charliteral">&#39;B&#39;</span>, address))  <span class="comment">// Set object tracking mode</span>
<a name="l00123"></a>00123     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00124"></a>00124   wait1Msec(500);
<a name="l00125"></a>00125 
<a name="l00126"></a>00126   <span class="keywordflow">if</span> (!<a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(link,<span class="charliteral">&#39;E&#39;</span>, address))  <span class="comment">// Start object tracking</span>
<a name="l00127"></a>00127     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00128"></a>00128   wait1Msec(500);
<a name="l00129"></a>00129 
<a name="l00130"></a>00130   <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00131"></a>00131 }
<a name="l00132"></a>00132 
<a name="l00133"></a>00133 <span class="comment"></span>
<a name="l00134"></a>00134 <span class="comment">/**</span>
<a name="l00135"></a>00135 <span class="comment"> * This function initialises camera ready to track lines.</span>
<a name="l00136"></a>00136 <span class="comment"> * @param link the sensor port number</span>
<a name="l00137"></a>00137 <span class="comment"> * @param address the I2C address to use, optional, defaults to 0x02</span>
<a name="l00138"></a>00138 <span class="comment"> * @return true if no error occured, false if it did</span>
<a name="l00139"></a>00139 <span class="comment"> */</span>
<a name="l00140"></a><a class="code" href="group__nxtcam.html#ga47cb3406f218eeb9e9711d0a7184e410">00140</a> <span class="keywordtype">bool</span> <a class="code" href="group__nxtcam.html#ga47cb3406f218eeb9e9711d0a7184e410">NXTCAMinitTL</a>(tSensors link, ubyte address) {
<a name="l00141"></a>00141   <span class="keywordflow">if</span> (!<a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(link, <span class="charliteral">&#39;D&#39;</span>, address)) <span class="comment">// Stop object tracking</span>
<a name="l00142"></a>00142     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00143"></a>00143   wait1Msec(500);
<a name="l00144"></a>00144 
<a name="l00145"></a>00145   <span class="keywordflow">if</span> (!<a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(link, <span class="charliteral">&#39;X&#39;</span>, address)) <span class="comment">// Do not sort objects</span>
<a name="l00146"></a>00146     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00147"></a>00147   wait1Msec(500);
<a name="l00148"></a>00148 
<a name="l00149"></a>00149   <span class="keywordflow">if</span> (!<a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(link,<span class="charliteral">&#39;L&#39;</span>, address))  <span class="comment">// Set tracking line mode</span>
<a name="l00150"></a>00150     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00151"></a>00151   wait1Msec(500);
<a name="l00152"></a>00152 
<a name="l00153"></a>00153   <span class="keywordflow">if</span> (!<a class="code" href="group__nxtcam.html#ga37e1ce7e64e571b7c3480404462d67e7">_camera_cmd</a>(link,<span class="charliteral">&#39;E&#39;</span>, address))  <span class="comment">// Start tracking</span>
<a name="l00154"></a>00154     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00155"></a>00155 
<a name="l00156"></a>00156   wait1Msec(500);
<a name="l00157"></a>00157   <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00158"></a>00158 }
<a name="l00159"></a>00159 
<a name="l00160"></a>00160 <span class="comment"></span>
<a name="l00161"></a>00161 <span class="comment">/**</span>
<a name="l00162"></a>00162 <span class="comment"> * This function fetches the blob data from the camera and merges the colliding ones.</span>
<a name="l00163"></a>00163 <span class="comment"> * @param link the sensor port number</span>
<a name="l00164"></a>00164 <span class="comment"> * @param blobs the array of blobs</span>
<a name="l00165"></a>00165 <span class="comment"> * @param mergeBlobs whether or not to merge the colliding blobs</span>
<a name="l00166"></a>00166 <span class="comment"> * @param address the I2C address to use, optional, defaults to 0x02</span>
<a name="l00167"></a>00167 <span class="comment"> * @return the number of blobs detected, -1 if an error occurred</span>
<a name="l00168"></a>00168 <span class="comment"> */</span>
<a name="l00169"></a><a class="code" href="group__nxtcam.html#ga5259766d6514ee73a08421ac88e77b12">00169</a> <span class="keywordtype">int</span> <a class="code" href="group__nxtcam.html#ga5259766d6514ee73a08421ac88e77b12">NXTCAMgetBlobs</a>(tSensors link, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs, <span class="keywordtype">bool</span> mergeBlobs, ubyte address) {
<a name="l00170"></a>00170   <span class="keywordtype">int</span> _nblobs = <a class="code" href="group__nxtcam.html#ga5259766d6514ee73a08421ac88e77b12">NXTCAMgetBlobs</a>(link, blobs, address);
<a name="l00171"></a>00171   <span class="keywordflow">if</span> (mergeBlobs == <span class="keyword">true</span>)
<a name="l00172"></a>00172     <span class="keywordflow">return</span> <a class="code" href="group__nxtcam.html#ga444410109e39138d52242c0570b5d0f9">_merge</a>(_nblobs, blobs);
<a name="l00173"></a>00173   <span class="keywordflow">return</span> _nblobs;
<a name="l00174"></a>00174 }
<a name="l00175"></a>00175 <span class="comment"></span>
<a name="l00176"></a>00176 <span class="comment">/**</span>
<a name="l00177"></a>00177 <span class="comment"> * This function fetches the blob data from the camera.</span>
<a name="l00178"></a>00178 <span class="comment"> * @param link the sensor port number</span>
<a name="l00179"></a>00179 <span class="comment"> * @param blobs the array of blobs</span>
<a name="l00180"></a>00180 <span class="comment"> * @param address the I2C address to use, optional, defaults to 0x02</span>
<a name="l00181"></a>00181 <span class="comment"> * @return the number of blobs detected, -1 if an error occurred</span>
<a name="l00182"></a>00182 <span class="comment"> */</span>
<a name="l00183"></a><a class="code" href="group__nxtcam.html#gaad3f1f7dadba3f8386b4756ce9f1888f">00183</a> <span class="keywordtype">int</span> <a class="code" href="group__nxtcam.html#ga5259766d6514ee73a08421ac88e77b12">NXTCAMgetBlobs</a>(tSensors link, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs, ubyte address) {
<a name="l00184"></a>00184   <span class="keywordtype">int</span> _nblobs = 0;
<a name="l00185"></a>00185 
<a name="l00186"></a>00186   <span class="comment">// clear the array used for the blobs</span>
<a name="l00187"></a>00187   memset(blobs, 0, <span class="keyword">sizeof</span>(<a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a>));
<a name="l00188"></a>00188 
<a name="l00189"></a>00189   <span class="comment">// Request number of blobs from the count register</span>
<a name="l00190"></a>00190   <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[0] = 2;                 <span class="comment">// Message size</span>
<a name="l00191"></a>00191   <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[1] = address;           <span class="comment">// I2C Address</span>
<a name="l00192"></a>00192   <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[2] = <a class="code" href="group__nxtcam.html#ga87f344485cd003c785b54959c883b924">NXTCAM_COUNT_REG</a>;  <span class="comment">// Register used to hold number of blobs detected</span>
<a name="l00193"></a>00193 
<a name="l00194"></a>00194   <span class="keywordflow">if</span> (!<a class="code" href="group__common.html#gad02e06f83b264ee6fc7df41eb4712080">writeI2C</a>(link, <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>, <a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">NXTCAM_I2CReply</a>, 1))
<a name="l00195"></a>00195     <span class="keywordflow">return</span> -1;
<a name="l00196"></a>00196 
<a name="l00197"></a>00197   _nblobs = <a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">NXTCAM_I2CReply</a>[0];
<a name="l00198"></a>00198   <span class="keywordflow">if</span> (_nblobs &gt; <a class="code" href="group__nxtcam.html#ga3104c35d33ecdfdcaa2af97f1691df8a">MAX_BLOBS</a>) {
<a name="l00199"></a>00199     <span class="keywordflow">return</span> -1;
<a name="l00200"></a>00200   }
<a name="l00201"></a>00201 
<a name="l00202"></a>00202   <span class="comment">// Get nblobs of blob data from the camera</span>
<a name="l00203"></a>00203   <span class="keywordflow">for</span> (<span class="keywordtype">int</span> _i = 0; _i &lt; _nblobs; _i++) {
<a name="l00204"></a>00204 
<a name="l00205"></a>00205     <span class="comment">// Request blob data</span>
<a name="l00206"></a>00206     <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[0] = 2;                         <span class="comment">// Message size</span>
<a name="l00207"></a>00207     <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[1] = address;           <span class="comment">// I2C Address</span>
<a name="l00208"></a>00208     <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>[2] = <a class="code" href="group__nxtcam.html#gadd6a460c504511474edf8a6f4e9687a6">NXTCAM_DATA_REG</a> + _i * 5;  <span class="comment">// Register containing data pertaining to blob</span>
<a name="l00209"></a>00209 
<a name="l00210"></a>00210     <span class="keywordflow">if</span> (!<a class="code" href="group__common.html#gad02e06f83b264ee6fc7df41eb4712080">writeI2C</a>(link, <a class="code" href="group__nxtcam.html#gaa1d603a296a06a71f002d0535d5e14fb">NXTCAM_I2CRequest</a>, <a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">NXTCAM_I2CReply</a>, 5))
<a name="l00211"></a>00211       <span class="keywordflow">return</span> -1;
<a name="l00212"></a>00212 
<a name="l00213"></a>00213     <span class="comment">// Put the I2C data into the blob</span>
<a name="l00214"></a>00214     blobs[_i].colour    = (int)<a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">NXTCAM_I2CReply</a>[0];
<a name="l00215"></a>00215     blobs[_i].x1        = (int)<a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">NXTCAM_I2CReply</a>[1];
<a name="l00216"></a>00216     blobs[_i].y1        = (int)<a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">NXTCAM_I2CReply</a>[2];
<a name="l00217"></a>00217     blobs[_i].x2        = (int)<a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">NXTCAM_I2CReply</a>[3];
<a name="l00218"></a>00218     blobs[_i].y2        = (int)<a class="code" href="group__nxtcam.html#gabb37063af562082aa6d0b0bf50278b6e">NXTCAM_I2CReply</a>[4];
<a name="l00219"></a>00219     blobs[_i].size      = abs(blobs[_i].x2 - blobs[_i].x1) * abs(blobs[_i].y2 - blobs[_i].y1);
<a name="l00220"></a>00220   }
<a name="l00221"></a>00221   <span class="keywordflow">return</span> _nblobs;
<a name="l00222"></a>00222 }
<a name="l00223"></a>00223 <span class="comment"></span>
<a name="l00224"></a>00224 <span class="comment">/**</span>
<a name="l00225"></a>00225 <span class="comment"> * Go through the blobs and calls _mergeBlobs.</span>
<a name="l00226"></a>00226 <span class="comment"> *</span>
<a name="l00227"></a>00227 <span class="comment"> * Note: this is an internal function and should not be called directly.</span>
<a name="l00228"></a>00228 <span class="comment"> * @param nblobs the number of blobs</span>
<a name="l00229"></a>00229 <span class="comment"> * @param blobs the array of blobs</span>
<a name="l00230"></a>00230 <span class="comment"> * @return the number of blobs detected</span>
<a name="l00231"></a>00231 <span class="comment"> */</span>
<a name="l00232"></a><a class="code" href="group__nxtcam.html#ga444410109e39138d52242c0570b5d0f9">00232</a> <span class="keywordtype">int</span> <a class="code" href="group__nxtcam.html#ga444410109e39138d52242c0570b5d0f9">_merge</a>(<span class="keywordtype">int</span> nblobs, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs) {
<a name="l00233"></a>00233   <a class="code" href="group__nxtcam.html#ga02fd1071ae1b5e8665557ab086c33112">still_merging</a> = <span class="keyword">true</span>;
<a name="l00234"></a>00234   <span class="keywordflow">while</span> (<a class="code" href="group__nxtcam.html#ga02fd1071ae1b5e8665557ab086c33112">still_merging</a> == <span class="keyword">true</span>) {
<a name="l00235"></a>00235     <a class="code" href="group__nxtcam.html#ga02fd1071ae1b5e8665557ab086c33112">still_merging</a> = <span class="keyword">false</span>;
<a name="l00236"></a>00236     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; nblobs; i++) {
<a name="l00237"></a>00237       <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = i+1; j &lt; nblobs; j++) {
<a name="l00238"></a>00238         nblobs = <a class="code" href="group__nxtcam.html#gadb70d2a4b3e24685060d2bcf95d9b473">_mergeBlobs</a>(i,j, nblobs, blobs);
<a name="l00239"></a>00239       }
<a name="l00240"></a>00240     }
<a name="l00241"></a>00241   }
<a name="l00242"></a>00242   <span class="keywordflow">return</span> nblobs;
<a name="l00243"></a>00243 }
<a name="l00244"></a>00244 <span class="comment"></span>
<a name="l00245"></a>00245 <span class="comment">/**</span>
<a name="l00246"></a>00246 <span class="comment"> * Check if two blobs can be merged into one.  blob1 will be replaced with the</span>
<a name="l00247"></a>00247 <span class="comment"> * new merged blob and blob2 will be destroyed.</span>
<a name="l00248"></a>00248 <span class="comment"> *</span>
<a name="l00249"></a>00249 <span class="comment"> * Note: this is an internal function and should not be called directly.</span>
<a name="l00250"></a>00250 <span class="comment"> * @param blob1 the index number of the first blob</span>
<a name="l00251"></a>00251 <span class="comment"> * @param blob2 the index number of the second blob</span>
<a name="l00252"></a>00252 <span class="comment"> * @param nblobs the number of blobs</span>
<a name="l00253"></a>00253 <span class="comment"> * @param blobs the array of blobs</span>
<a name="l00254"></a>00254 <span class="comment"> * @return the number of blobs detected</span>
<a name="l00255"></a>00255 <span class="comment"> */</span>
<a name="l00256"></a><a class="code" href="group__nxtcam.html#gadb70d2a4b3e24685060d2bcf95d9b473">00256</a> <span class="keywordtype">int</span> <a class="code" href="group__nxtcam.html#gadb70d2a4b3e24685060d2bcf95d9b473">_mergeBlobs</a>(<span class="keywordtype">int</span> blob1, <span class="keywordtype">int</span> blob2, <span class="keywordtype">int</span> nblobs, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs) {
<a name="l00257"></a>00257   <span class="keywordtype">int</span> _blob1_center;
<a name="l00258"></a>00258   <span class="keywordtype">int</span> _blob2_center;
<a name="l00259"></a>00259   <span class="keywordtype">int</span> _blob1_proj;
<a name="l00260"></a>00260   <span class="keywordtype">int</span> _blob2_proj;
<a name="l00261"></a>00261   <span class="keywordtype">bool</span> _overlapx = <span class="keyword">false</span>;
<a name="l00262"></a>00262   <span class="keywordtype">bool</span> _overlapy = <span class="keyword">false</span>;
<a name="l00263"></a>00263 
<a name="l00264"></a>00264   <span class="comment">// If either pf the blobs are size 0, just skip them</span>
<a name="l00265"></a>00265   <span class="keywordflow">if</span> (blobs[blob1].size == 0 || blobs[blob2].size == 0)
<a name="l00266"></a>00266     <span class="keywordflow">return</span> nblobs;
<a name="l00267"></a>00267 
<a name="l00268"></a>00268   <span class="comment">// If the colours don&#39;t match, don&#39;t _merge them</span>
<a name="l00269"></a>00269   <span class="keywordflow">if</span> (blobs[blob1].colour != blobs[blob2].colour)
<a name="l00270"></a>00270     <span class="keywordflow">return</span> nblobs;
<a name="l00271"></a>00271 
<a name="l00272"></a>00272   <span class="comment">// Find the center of the top sides of each blob and their projections onto</span>
<a name="l00273"></a>00273   <span class="comment">// the X plane from center to right corner</span>
<a name="l00274"></a>00274   _blob1_center = <a class="code" href="group__nxtcam.html#gaac63b3236275d61b690cb17ea7634072">SIDE_CENTER</a>(blobs[blob1].x1, blobs[blob1].x2);
<a name="l00275"></a>00275   _blob2_center = <a class="code" href="group__nxtcam.html#gaac63b3236275d61b690cb17ea7634072">SIDE_CENTER</a>(blobs[blob2].x1, blobs[blob2].x2);
<a name="l00276"></a>00276   _blob1_proj = blobs[blob1].x2 - _blob1_center;
<a name="l00277"></a>00277   _blob2_proj = blobs[blob2].x2 - _blob2_center;
<a name="l00278"></a>00278 
<a name="l00279"></a>00279   <span class="comment">// Check if the projections overlap</span>
<a name="l00280"></a>00280   <span class="keywordflow">if</span> ((_blob1_proj + _blob2_proj) &gt; abs(_blob1_center - _blob2_center))
<a name="l00281"></a>00281     _overlapx = <span class="keyword">true</span>;
<a name="l00282"></a>00282 
<a name="l00283"></a>00283   <span class="comment">// Find the center of the left sides of each blob and their projections onto</span>
<a name="l00284"></a>00284   <span class="comment">// the Y plane from center to bottom corner</span>
<a name="l00285"></a>00285   _blob1_center = <a class="code" href="group__nxtcam.html#gaac63b3236275d61b690cb17ea7634072">SIDE_CENTER</a>(blobs[blob1].y1, blobs[blob1].y2);
<a name="l00286"></a>00286   _blob2_center = <a class="code" href="group__nxtcam.html#gaac63b3236275d61b690cb17ea7634072">SIDE_CENTER</a>(blobs[blob2].y1, blobs[blob2].y2);
<a name="l00287"></a>00287   _blob1_proj = blobs[blob1].y2 - _blob1_center;
<a name="l00288"></a>00288   _blob2_proj = blobs[blob2].y2 - _blob2_center;
<a name="l00289"></a>00289 
<a name="l00290"></a>00290   <span class="comment">// Check if the projections overlap</span>
<a name="l00291"></a>00291   <span class="keywordflow">if</span> ((_blob1_proj + _blob2_proj) &gt; abs(_blob1_center - _blob2_center))
<a name="l00292"></a>00292     _overlapy = <span class="keyword">true</span>;
<a name="l00293"></a>00293 
<a name="l00294"></a>00294   <span class="comment">// If both projections are overlapping, then the blobs are _merged.</span>
<a name="l00295"></a>00295   <span class="comment">// Find the smallest bounding box for both blobs and replace blob1</span>
<a name="l00296"></a>00296   <span class="comment">// with this new information.  Blob2 gets set to 0 size and</span>
<a name="l00297"></a>00297   <span class="comment">// recursively replaced by the next in line.</span>
<a name="l00298"></a>00298   <span class="keywordflow">if</span> ((_overlapx == <span class="keyword">true</span>) &amp;&amp; (_overlapy == <span class="keyword">true</span>)) {
<a name="l00299"></a>00299     blobs[blob1].x1 = <a class="code" href="group__common.html#gaaff7fb3e43cf77514fd0fec17012e360">min2</a>(blobs[blob1].x1, blobs[blob2].x1);
<a name="l00300"></a>00300     blobs[blob1].y1 = <a class="code" href="group__common.html#gaaff7fb3e43cf77514fd0fec17012e360">min2</a>(blobs[blob1].y1, blobs[blob2].y1);
<a name="l00301"></a>00301     blobs[blob1].x2 = <a class="code" href="group__common.html#ga59934691af1d56f6e8f6d46c781bbcb5">max2</a>(blobs[blob1].x2, blobs[blob2].x2);
<a name="l00302"></a>00302     blobs[blob1].y2 = <a class="code" href="group__common.html#ga59934691af1d56f6e8f6d46c781bbcb5">max2</a>(blobs[blob1].y2, blobs[blob2].y2);
<a name="l00303"></a>00303     blobs[blob1].size = abs(blobs[blob1].x2 - blobs[blob1].x1) * abs(blobs[blob1].y2 - blobs[blob1].y1);
<a name="l00304"></a>00304 
<a name="l00305"></a>00305     <span class="keywordflow">for</span> (<span class="keywordtype">int</span> _i = blob2; _i &lt; nblobs - 1; _i++) {
<a name="l00306"></a>00306       memcpy(blobs[_i], blobs[_i+1], <span class="keyword">sizeof</span>(<a class="code" href="structblob.html">blob</a>));
<a name="l00307"></a>00307     }
<a name="l00308"></a>00308 
<a name="l00309"></a>00309     nblobs--;
<a name="l00310"></a>00310     memset(blobs[nblobs], 0, <span class="keyword">sizeof</span>(<a class="code" href="structblob.html">blob</a>));
<a name="l00311"></a>00311     <a class="code" href="group__nxtcam.html#ga02fd1071ae1b5e8665557ab086c33112">still_merging</a> = <span class="keyword">true</span>;
<a name="l00312"></a>00312   }
<a name="l00313"></a>00313   <a class="code" href="group__nxtcam.html#ga1d6f5eddd1d996914310d608d031664e">_sortBlobs</a>(nblobs, blobs);
<a name="l00314"></a>00314 
<a name="l00315"></a>00315   <span class="comment">// return the new number of blobs</span>
<a name="l00316"></a>00316   <span class="keywordflow">return</span> nblobs;
<a name="l00317"></a>00317 }
<a name="l00318"></a>00318 <span class="comment"></span>
<a name="l00319"></a>00319 <span class="comment">/**</span>
<a name="l00320"></a>00320 <span class="comment"> * This function sorts the blobs in the array using insertion sort.</span>
<a name="l00321"></a>00321 <span class="comment"> *</span>
<a name="l00322"></a>00322 <span class="comment"> * Note: this is an internal function and should not be called directly.</span>
<a name="l00323"></a>00323 <span class="comment"> * @param nblobs the number of blobs</span>
<a name="l00324"></a>00324 <span class="comment"> * @param blobs the array of blobs</span>
<a name="l00325"></a>00325 <span class="comment"> */</span>
<a name="l00326"></a><a class="code" href="group__nxtcam.html#ga1d6f5eddd1d996914310d608d031664e">00326</a> <span class="keywordtype">void</span> <a class="code" href="group__nxtcam.html#ga1d6f5eddd1d996914310d608d031664e">_sortBlobs</a>(<span class="keywordtype">int</span> nblobs, <a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs) {
<a name="l00327"></a>00327   <a class="code" href="structblob.html">blob</a> _tmpBlob;
<a name="l00328"></a>00328   <span class="keywordtype">int</span> i, j;
<a name="l00329"></a>00329 
<a name="l00330"></a>00330   <span class="comment">// Outer boundary definition</span>
<a name="l00331"></a>00331   <span class="keywordflow">for</span> (i = 1; i &lt; nblobs; i++) {
<a name="l00332"></a>00332     <span class="keywordflow">if</span> (blobs[i-1].size &gt;= blobs[i].size )
<a name="l00333"></a>00333       <span class="keywordflow">continue</span>;
<a name="l00334"></a>00334 
<a name="l00335"></a>00335     memcpy(_tmpBlob, blobs[i], <span class="keyword">sizeof</span>(<a class="code" href="structblob.html">blob</a>));
<a name="l00336"></a>00336 
<a name="l00337"></a>00337     <span class="comment">// inner loop: elements shifted down until insertion point found</span>
<a name="l00338"></a>00338     <span class="keywordflow">for</span> (j = i-1; j &gt;= 0; j--) {
<a name="l00339"></a>00339       <span class="keywordflow">if</span> ( blobs[j].size &gt;= _tmpBlob.<a class="code" href="structblob.html#a439227feff9d7f55384e8780cfc2eb82">size</a> )
<a name="l00340"></a>00340         <span class="keywordflow">break</span>;
<a name="l00341"></a>00341       memcpy(blobs[j+1], blobs[j], <span class="keyword">sizeof</span>(<a class="code" href="structblob.html">blob</a>));
<a name="l00342"></a>00342     }
<a name="l00343"></a>00343     <span class="comment">// insert the blob</span>
<a name="l00344"></a>00344     memcpy(blobs[j+1], _tmpBlob, <span class="keyword">sizeof</span>(<a class="code" href="structblob.html">blob</a>));
<a name="l00345"></a>00345   }
<a name="l00346"></a>00346 }
<a name="l00347"></a>00347 <span class="comment"></span>
<a name="l00348"></a>00348 <span class="comment">/**</span>
<a name="l00349"></a>00349 <span class="comment"> * Calculate the average center of all the blobs of a specific colour.</span>
<a name="l00350"></a>00350 <span class="comment"> *</span>
<a name="l00351"></a>00351 <span class="comment"> * Note: this is an experimental function and may not function properly yet.</span>
<a name="l00352"></a>00352 <span class="comment"> * @param blobs the array of blobs</span>
<a name="l00353"></a>00353 <span class="comment"> * @param nblobs the number of blobs</span>
<a name="l00354"></a>00354 <span class="comment"> * @param colourindex the colour of the blobs of which the average center is to be calculated</span>
<a name="l00355"></a>00355 <span class="comment"> * @param x x-coordinate of the center</span>
<a name="l00356"></a>00356 <span class="comment"> * @param y y-coordinate of the center</span>
<a name="l00357"></a>00357 <span class="comment"> */</span>
<a name="l00358"></a><a class="code" href="group__nxtcam.html#ga177c2ae2e5fcc598179d2289b3a420df">00358</a> <span class="keywordtype">void</span> <a class="code" href="group__nxtcam.html#ga177c2ae2e5fcc598179d2289b3a420df">NXTCAMgetAverageCenter</a>(<a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs, <span class="keywordtype">int</span> nblobs, <span class="keywordtype">int</span> colourindex, <span class="keywordtype">int</span> &amp;x, <span class="keywordtype">int</span> &amp;y) {
<a name="l00359"></a>00359   <span class="keywordtype">long</span> _totalX = 0;
<a name="l00360"></a>00360   <span class="keywordtype">long</span> _totalY = 0;
<a name="l00361"></a>00361   <span class="keywordtype">int</span> _counter = 0;
<a name="l00362"></a>00362 
<a name="l00363"></a>00363   <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; nblobs; i++){
<a name="l00364"></a>00364     <span class="keywordflow">if</span> ((blobs[i].colour == colourindex) &amp;&amp; (blobs[i].size &gt; 400)) {
<a name="l00365"></a>00365       _totalX += <a class="code" href="group__nxtcam.html#gaac63b3236275d61b690cb17ea7634072">SIDE_CENTER</a>(blobs[i].x1, blobs[i].x2);
<a name="l00366"></a>00366       _totalY += <a class="code" href="group__nxtcam.html#gaac63b3236275d61b690cb17ea7634072">SIDE_CENTER</a>(blobs[i].y1, blobs[i].y2);
<a name="l00367"></a>00367       _counter++;
<a name="l00368"></a>00368     }
<a name="l00369"></a>00369   }
<a name="l00370"></a>00370   x = _totalX / (_counter - 1);
<a name="l00371"></a>00371   y = _totalY / (_counter  -1);
<a name="l00372"></a>00372 }
<a name="l00373"></a>00373 
<a name="l00374"></a>00374 <span class="comment"></span>
<a name="l00375"></a>00375 <span class="comment">/**</span>
<a name="l00376"></a>00376 <span class="comment"> * Calculate the center of a specified blob.</span>
<a name="l00377"></a>00377 <span class="comment"> *</span>
<a name="l00378"></a>00378 <span class="comment"> * Note: this is an experimental function and may not function properly yet.</span>
<a name="l00379"></a>00379 <span class="comment"> * @param blobs the array of blobs</span>
<a name="l00380"></a>00380 <span class="comment"> * @param index the blob that you want to calculate the center of</span>
<a name="l00381"></a>00381 <span class="comment"> * @param x x-coordinate of the center</span>
<a name="l00382"></a>00382 <span class="comment"> * @param y y-coordinate of the center</span>
<a name="l00383"></a>00383 <span class="comment"> */</span>
<a name="l00384"></a><a class="code" href="group__nxtcam.html#ga6ca36236d45c9cdc7bbc56f266bdf6c6">00384</a> <span class="keywordtype">void</span> <a class="code" href="group__nxtcam.html#ga6ca36236d45c9cdc7bbc56f266bdf6c6">NXTCAMgetCenter</a>(<a class="code" href="group__nxtcam.html#gaa3db0dd75c1de6e6a73579910a97f8e8">blob_array</a> &amp;blobs, <span class="keywordtype">int</span> index, <span class="keywordtype">int</span> &amp;x, <span class="keywordtype">int</span> &amp;y) {
<a name="l00385"></a>00385   x = <a class="code" href="group__nxtcam.html#gaac63b3236275d61b690cb17ea7634072">SIDE_CENTER</a>(blobs[index].x1, blobs[index].x2);
<a name="l00386"></a>00386   y = <a class="code" href="group__nxtcam.html#gaac63b3236275d61b690cb17ea7634072">SIDE_CENTER</a>(blobs[index].y1, blobs[index].y2);
<a name="l00387"></a>00387 }
<a name="l00388"></a>00388 
<a name="l00389"></a>00389 <span class="preprocessor">#endif // __NXTCAM_H__</span>
<a name="l00390"></a>00390 <span class="preprocessor"></span>
<a name="l00391"></a>00391 <span class="comment">/*</span>
<a name="l00392"></a>00392 <span class="comment"> * $Id: mindsensors-nxtcam.h 133 2013-03-10 15:15:38Z xander $</span>
<a name="l00393"></a>00393 <span class="comment"> */</span>
<a name="l00394"></a>00394 <span class="comment">/* @} */</span>
<a name="l00395"></a>00395 <span class="comment">/* @} */</span>
</pre></div></div>
</div>
<hr class="footer"/><address class="footer"><small>Generated on Wed Mar 13 2013 19:56:43 for ROBOTC Drivers by&#160;
<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.7.2 </small></address>
</body>
</html>
